/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.grt192.controller.breakaway.auto;

import com.grt192.core.StepController;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
import com.grt192.mechanism.breakaway.KickerOmega;
import com.grt192.mechanism.breakaway.Roller;
import com.grt192.sensor.GRTSwitch;

/**
 * 
 * @author GRT
 */
public class BreakawayAutoDeadReckoningController extends StepController
		implements SensorChangeListener {

	public static final int OP = 0;
	public static final int VALUE = 1;
	
	public static final double ON = 1.0;

	public static final int MOVE = 0;
	public static final int TURN = 1;
	public static final int ROLLERS = 2;
	public static final int SHOOT = 3;
	public static final int DELAY = 4;

	public static final int DELAY_TIME = 10000; // ms;

	public static final int DIST_PER_COUNT = 1;

	public static final double[][] MATCH_COMMANDS = { { DELAY, DELAY_TIME },
			{ ROLLERS, ON }, { MOVE, 36 }, { SHOOT }, { ROLLERS, 0.0 } };
	public static final double SPEED = -.5;
	private int commandNumber = 0;
	private int count = 0;
	private int time = 0;
	private boolean delaying = false;
	private double[][] commands;

	private GRTBreakawayRobotBase rb;
	private Roller rollers;
	private KickerOmega kicker;
	private int state;

	public BreakawayAutoDeadReckoningController(GRTBreakawayRobotBase rb,
			Roller roller, KickerOmega kicker) {
		addMechanism("RobotBase", rb);
		addMechanism("Roller", roller);
		addMechanism("Kicker", kicker);
		commands = MATCH_COMMANDS;
		state = (int) commands[commandNumber][OP];
		this.rb = rb;
		this.kicker = kicker;
		this.rollers = rollers;
		((GRTSwitch) kicker.getSensor("LimitSwitch"))
				.addSensorChangeListener(this);
	}

	public void act() {
		if (commandNumber < commands.length) {
			/*
			 * if (state != (int) commands[commandNumber][OP]) {
			 * System.out.println("changing state"); }
			 */
			state = (int) commands[commandNumber][OP];
			switch (state) {
			case MOVE: {
				System.out.println("Moving "+time);
				if (move(commands[commandNumber][VALUE])) {
					commandNumber++;
				}
				break;
			}
			case ROLLERS: {
				if (commands[commandNumber][VALUE] == ON) {
					turnOnRollers();
				} else {
					turnOffRollers();
				}
				commandNumber++;
				break;
			}
			case SHOOT: {
				if (!kicker.isKicking())
					// shoot();
					break;

			}
			case DELAY: {
				if (!delaying) {
					time = 0;
					delaying = true;
				} else if (time >= commands[commandNumber][VALUE]) {
					delaying = false;
					commandNumber++;
				} else
					System.out.println("Waiting "+time);

			}
			}
		}
		time += 50;
	}

	public boolean move(double dist) {
		if (dist <= count * DIST_PER_COUNT) {
			rb.stop();
			count = 0;
			return true;
		} else {
			rb.tankDrive(SPEED, SPEED);
			count++;
			return false;
		}
	}

	public void turnOnRollers() {
		((Roller) getMechanism("Roller")).grab();
	}

	public void turnOffRollers() {
		rollers.stop();
	}

	public void shoot() {
		((KickerOmega) getMechanism("Kicker")).kick();
	}

	public void sensorStateChanged(SensorEvent e, String key) {
		boolean switchHit = e.getData(key) == KickerOmega.SWITCH_HIT;
		System.out.println("Limit Switch hit");
		if (switchHit && kicker.isKicking()) {
			kicker.stopKick();
			commandNumber++;
		}
	}

}
